from numpy import *

def magnitude(vector):
    total = 0
    for v in vector:
        total += v**2

    return sqrt(total)

def normalize(vector):

    rv = vector
    m = magnitude(vector)
    if m != 0:
        rv = vector/m

    return rv


def cross(u, v):
    return array([  u[1]*v[2] - u[2]*v[1],
                    u[2]*v[0] - u[0]*v[2],
                    u[0]*v[1] - u[1]*v[0]   ])


def rotation_matrix(angles):

    (ax, ay, az) = angles
    ax = math.radians(ax)
    ay = math.radians(ay)
    az = math.radians(az)

    sx = math.sin(ax)
    cx = math.cos(ax)

    sy = math.sin(ay)
    cy = math.cos(ay)
        
    sz = math.sin(az)
    cz = math.cos(az)

    rx = array(((1.0, 0.0, 0.0),
                (0.0, cx, -sx),
                (0.0, sx, cx) ))
    ry = array(((cy, 0.0, sy),
                (0.0, 1.0, 0.0),
                (-sy, 0.0, cy) ))
    rz = array(((cz, -sz, 0.0),
                (sz, cz, 0.0),
                (0.0, 0.0, 1.0) ))

    r = dot(rx, ry)
    r = dot(r, rz)

    return r



